#include <ros/ros.h>
#include <iostream>
#include <stdio.h>

#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>
#include <sensor_msgs/CameraInfo.h>
#include "maker_binocular.h"

int main(int argc,  char ** argv)
{ 
    ros::init(argc, argv, "maker_binocular");
    
    ros::NodeHandle nh;
    
    ros::Publisher  left_image_pub =  nh.advertise<sensor_msgs::Image>("camera/left/image_raw", 10);
    ros::Publisher right_image_pub = nh.advertise<sensor_msgs::Image>("camera/right/image_raw", 10);
    
    ros::Publisher  left_rgb_image_pub =  nh.advertise<sensor_msgs::Image>("left/image_color", 10);
    ros::Publisher right_rgb_image_pub = nh.advertise<sensor_msgs::Image>("right/image_color", 10);
    
    ros::Publisher left_camera_info_pub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info",  10);
    ros::Publisher right_camera_info_pub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info",  10);
    
    // set up stereo camera info
    sensor_msgs::CameraInfo left_camera_info;
    sensor_msgs::CameraInfo right_camera_info;
    
    left_camera_info.distortion_model = "plumb_bob";
    right_camera_info.distortion_model = "plumb_bob";
        
/*
    // left camera
    left_camera_info.height = 480;
    left_camera_info.width = 640; 
    left_camera_info.D.push_back(0.144948);
    left_camera_info.D.push_back(-0.353241);
    left_camera_info.D.push_back(-0.002081);
    left_camera_info.D.push_back(0.007011);
    left_camera_info.D.push_back(0.000000);
     
    left_camera_info.K = {701.415048, 0.0, 325.936563, 0.0, 700.805741, 235.991749, 0.0, 0.0, 1.0};
    left_camera_info.R = {0.999240, -0.000872, 0.038958, 0.001172, 0.999970, -0.007668, -0.038950, 0.007708, 0.999211};
    left_camera_info.P = {768.658277, 0, 280.543400, 0,  0, 768.658277, 244.094297, 0,  0, 0, 1, 0};
    left_camera_info.binning_x = 1;
    left_camera_info.binning_y = 1;
    
    // right camera
    right_camera_info.D.push_back(0.065938);
    right_camera_info.D.push_back(0.026545);
    right_camera_info.D.push_back(0.000794);
    right_camera_info.D.push_back(0.004122); 
    right_camera_info.D.push_back(0.000000);
    
    right_camera_info.K = {702.170323, 0.0, 300.267293, 0.0, 702.301011, 253.483819, 0.0, 0.0, 1};
    right_camera_info.R = {0.998917, -0.004869, 0.046274, 0.004513,  0.999959, 0.007796, -0.046310, -0.007579, 0.998898};
    right_camera_info.P = {768.658277, 0, 280.543400 , -64.649990, 0, 768.658277, 244.094297, 0, 0, 0, 1, 0}; 
    right_camera_info.binning_x = 1;
    right_camera_info.binning_y = 1;
*/
    int counter = 0;
    sensor_msgs::Image ros_left_image,  ros_right_image;

    makerbinocular m_makerbinocular;

    cv::Mat left_image(480, 640, CV_8UC1, cv::Scalar(0));
    cv::Mat right_image(480, 640, CV_8UC1, cv::Scalar(0));

    cv::Mat left_image_rgb(480, 640, CV_8UC3, cv::Scalar(0));
    cv::Mat right_image_rgb(480, 640, CV_8UC3, cv::Scalar(0));
    ////////////////

    cv_bridge::CvImage img_bridge;
    std_msgs::Header header;

    header.frame_id = "/camera_link";

    ros::Rate loop_rate(30);
    if (m_makerbinocular.is_initialized())
    {
        while (ros::ok())
        {
            m_makerbinocular.get_frame(left_image, right_image);
            
            if (m_makerbinocular.new_frame_arrived())
            {
                header.seq = counter;
                header.stamp = ros::Time::now();

                // for gray image,  just publishe it
                if (true)//!m_makerbinocular.is_rgb())
                {
                    img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8,  left_image);
                    img_bridge.toImageMsg(ros_left_image);
                    left_image_pub.publish(ros_left_image);

                    img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8,  right_image);
                    img_bridge.toImageMsg(ros_right_image);
                    right_image_pub.publish(ros_right_image);
                }
                else
                {
                 //TODO:add rgb support   
                    cv::cvtColor(left_image, left_image_rgb,  CV_GRAY2RGB);
                    cv::cvtColor(right_image, right_image_rgb,  CV_GRAY2RGB);
                 
                }
                // give the content of stereo camera info
                left_camera_info.header = header;
                right_camera_info.header = header;

                left_camera_info_pub.publish(left_camera_info);
                right_camera_info_pub.publish(right_camera_info);

                cv::imshow("left image", left_image);
                cv::waitKey(1);
                cv::imshow("right image" ,  right_image);
                cv::waitKey(1);

                counter++;
            }
            loop_rate.sleep();
        }
    }

    return 0;
}
